// -*-C++-*-
/*!
 * @file  MapInfoSVC_impl.cpp
 * @brief Service implementation code of MapInfo.idl
 *
 */

#include "MapInfoSVC_impl.h"
#include <iostream>
#include <stdlib.h>
#include "map.h"


/*
 * Example implementational code for IDL interface MapInfo
 */
MapInfoSVC_impl::MapInfoSVC_impl(MapData* map_p)
{
  map = map_p;
}


MapInfoSVC_impl::~MapInfoSVC_impl()
{
  // Please add extra destructor code here.
}


/*
 * Methods corresponding to IDL attributes and operations
 */
CORBA::Boolean MapInfoSVC_impl::setFullGridMap(const RTC::TimedOctetSeq& mapdata,
					       CORBA::Short origin_coord_x, CORBA::Short origin_coord_y,
					       CORBA::Short width, CORBA::Short height,
					       CORBA::Double resolution)
{
  unsigned map_size = width*height;
  int i, j, focus;

  // サイズチェック
  if(map_size != mapdata.data.length()){
    std::cerr << "size of grid map from service port is irregular. " << MAX_OCCUPANCY_GRIDMAP_SIZE << "!=" << width << "*" << height << std::endl;
    return false;
  }

  // 地図のリセット
  release_maps(map);
  // メモリ確保
  if(map->ogmap == NULL)
    map->ogmap = (OccupancyGridMap *)realloc(map->ogmap, sizeof(OccupancyGridMap));

  // サイズ，解像度の格納
  map->ogmap->origin_x = origin_coord_x;
  map->ogmap->origin_y = origin_coord_y;
  map->ogmap->width = width;
  map->ogmap->height = height;
  map->ogmap->resolution = resolution;
  // 地図データの格納

  for(i=0; i<height; i++){
  
	  for(j=0; j<width; j++){
      
		  focus = width*i+j;

      
		  map->ogmap->data[focus] = mapdata.data[focus];
    
	  }
  }

  std::cout << "Received Grid Map data. " << width << "*" << height << "=" << map_size << "" << std::endl;

  return true;
}

CORBA::Boolean MapInfoSVC_impl::getFullGridMap(RTC::TimedOctetSeq& mapdata,
					       CORBA::Short& origin_coord_x, CORBA::Short& origin_coord_y,
					       CORBA::Short& width, CORBA::Short& height,
					       CORBA::Double& resolution)
{
  int i,j;
  unsigned map_size;
  int focus;

  std::cout << "Sending Grid Map data... ";

  if(map->ogmap != NULL){
    origin_coord_x = map->ogmap->origin_x;
    origin_coord_y = map->ogmap->origin_y;
    width  = map->ogmap->width;
    height = map->ogmap->height;
    resolution = map->ogmap->resolution;
    map_size = width*height;
    mapdata.data.length(map_size);

    for(i=0; i<height; i++){
		for(j=0; j<width; j++){
			focus = width*i+j;
			mapdata.data[focus] = map->ogmap->data[focus];
		}
    }

    std::cout << "done. " << width << "*" << height << "=" << map_size << "" << std::endl;

	return true;
  }

  // ラインマップからのコピー
  if(map->lmap != NULL){
    
    return true;
  }

  // ポイントマップからのコピー
  if(map->pmap != NULL){
    return true;
  }

  std::cout << "failed " << std::endl;
  return false;
}

CORBA::Boolean MapInfoSVC_impl::getPartialGridMap(CORBA::Short& partial_x, CORBA::Short& partial_y,
						  CORBA::Short& partial_width, CORBA::Short& partial_height,
						  RTC::TimedOctetSeq& partial_map)
{
  return false;
}


// End of example implementational code



